/*
* Copyright (c) Huawei Technologies Co., Ltd. 2022-2022. All rights reserved.
* Description: pcd_sender head file
*/
#ifndef PCD_SENDER_H
#define PCD_SENDER_H

#include <dirent.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <sys/time.h>
#include <sys/types.h>
#include <ctime>
#include <unistd.h>

#include <fstream>
#include <iostream>
#include <vector>

#include "yaml/haf_yaml.h"
#include "image_sender_base.h"
#include "ara/lidar/impl_type_lidarpointcloud.h"
#include "ara/lidar/impl_type_lidarpointfield.h"
#include "ara/lidar/lidarserviceinterface_common.h"
#include "ara/lidar/lidarserviceinterface_skeleton.h"
#include "util.h"

using namespace std;
const int16_t X_IN_POINTCLOUD = 0;
const int16_t Y_IN_POINTCLOUD = 4;
const int16_t Z_IN_POINTCLOUD = 8;
const int16_t INTENSITY_IN_POINTCLOUD = 28;
const int16_t X_IN_POINTCLOUD_SIMPLE = 0;
const int16_t Y_IN_POINTCLOUD_SIMPLE = 2;
const int16_t Z_IN_POINTCLOUD_SIMPLE = 4;
const int16_t INTENSITY_IN_POINTCLOUD_SIMPLE = 10;

namespace ara {
namespace lidar {
constexpr ara::com::ServiceIdentifierType LidarServiceInterface::ServiceIdentifier;
constexpr ara::com::ServiceVersionType LidarServiceInterface::ServiceVersion;
} // namespace lidar
} // namespace ara

using Skeleton_pcd = ara::lidar::skeleton::LidarServiceInterfaceSkeleton;

class PcdSender : public ImageSenderBase<Skeleton_pcd>
{
private:
    chrono::high_resolution_clock::time_point sec;//-t used
public:
    PcdSender() = default;
    PcdSender(uint32_t id, cv::Size s, std::string dir) : ImageSenderBase<Skeleton_pcd>(id, s, dir)
    {
        sec = chrono::high_resolution_clock::now();
    }
    ~PcdSender() = default;
    bool RegisterSender()
    {
        std::cout << "Begin register normal image sender." << std::endl;
        auto result = ImageSenderBase<Skeleton_pcd>::RegisterSender();
        std::cout << "Finished to register normal image sender." << std::endl;
        return result;
    }

    void FillImageAndSend(std::string imgPath, uint32_t seq, void *flag) override
    {
        std::string path = dir;
        std::string postfix = comPara.fileType;
        bool isLoop = comPara.loop;
        cout << "dir : " << dir << "\t" << "postfix :" << postfix << endl;
        std::vector<std::string> pcd_names;
        if (path.back() != '/') {
            path = path + string("/");
        }
        GetFilesFromDirectory(path, pcd_names);
        std::cout << "There are" << pcd_names.size() << " pcd files in " << path << std::endl;
        do {
            for (size_t i = 0; i < pcd_names.size(); i++) {
                const std::string pcd_name = pcd_names[i];
                const std::string pcd_source_path = path + pcd_name;
                std::cout << "The" << i << " pcd source path is: " << pcd_source_path << std::endl;
                pcl::PointCloud<pcl::PointXYZI>::Ptr lidarData(new pcl::PointCloud<pcl::PointXYZI>);
                if (pcl::io::loadPCDFile<pcl::PointXYZI>(pcd_source_path, *lidarData) == -1) {
                    std::cout << "load pcd failed" << std::endl;
                    continue;
                }
                std::cout << "Load local pcd successful, pointcloud size is: " << lidarData->width << ", " <<
                          lidarData->height << std::endl;
                std::stringstream str;
                str << "copy-" << pcd_name;
                pcl::PCDWriter writer;
#ifdef SAVE_SENDER_FILE
                writer.writeBinaryCompressed<pcl::PointXYZI>(str.str(), *lidarData);
#endif
                ara::lidar::LidarPointCloud tmp;
                tmp.header.frameId = pcd_name;
                if (comPara.transMode == "simple") {
                    tmp.pointStep = 13;
                } else {
                    tmp.pointStep = 32;//simple=13,standard=32
                }
                tmp.height = lidarData->height;
                tmp.width = lidarData->width;
                std::vector<uint8_t> pointCloudData;
                pointCloudData.resize(tmp.pointStep * tmp.height * tmp.width);
                uint8_t *pointCloud0 = &pointCloudData[0];
                uint8_t *pointCloudCurr = nullptr;
                int32_t loopCount = static_cast<int32_t>(tmp.height) * static_cast<int32_t>(tmp.width);
                const int32_t sliceSize = 7400000;
                int32_t maxNumOfPoints = (sliceSize - sizeof(tmp.header)) / tmp.pointStep;
                if (loopCount > maxNumOfPoints) {
                    std::cout << pcd_name << " send failed due to too many points,max points num is " << maxNumOfPoints << std::endl;
                    continue;
                }
                for (int32_t pointCount = 0; pointCount < loopCount; pointCount++) {
                    pointCloudCurr = pointCloud0 + tmp.pointStep * pointCount;
                    if (comPara.transMode == "simple") {
                        *(reinterpret_cast<int16_t *>(pointCloudCurr + X_IN_POINTCLOUD_SIMPLE)) = lidarData->points[pointCount].x * 100;
                        *(reinterpret_cast<int16_t *>(pointCloudCurr + Y_IN_POINTCLOUD_SIMPLE)) = lidarData->points[pointCount].y * 100;
                        *(reinterpret_cast<int16_t *>(pointCloudCurr + Z_IN_POINTCLOUD_SIMPLE)) = lidarData->points[pointCount].z * 100;
                        *(reinterpret_cast<uint8_t *>(pointCloudCurr + INTENSITY_IN_POINTCLOUD_SIMPLE)) =
                            lidarData->points[pointCount].intensity;
                    } else {
                        *(reinterpret_cast<float *>(pointCloudCurr + X_IN_POINTCLOUD)) = lidarData->points[pointCount].x;
                        *(reinterpret_cast<float *>(pointCloudCurr + Y_IN_POINTCLOUD)) = lidarData->points[pointCount].y;
                        *(reinterpret_cast<float *>(pointCloudCurr + Z_IN_POINTCLOUD)) = lidarData->points[pointCount].z;
                        *(reinterpret_cast<uint16_t *>(pointCloudCurr + INTENSITY_IN_POINTCLOUD)) =
                            lidarData->points[pointCount].intensity;
                    }
                }
                tmp.data.assign(pointCloudData.begin(), pointCloudData.end());
                if (comPara.time.empty()) {
                    timeval tv;
                    gettimeofday(&tv, 0);
                    tmp.header.stamp.sec = tv.tv_sec;
                    tmp.header.stamp.nsec = 1e3;
                } else {
                    time_t timeStamp = convertTimeStr2TimeStamp(comPara.time);
                    auto t1 = chrono::high_resolution_clock::now();
                    int64_t duration = (t1 - sec).count() / 1000000000.0;
                    tmp.header.stamp.sec = timeStamp + duration;
                    printf("timeStamp=%ld\n", timeStamp + duration);
                }
                tmp.header.seq = i;
                std::cout << "tmp.header.stamp.sec: " << tmp.header.stamp.sec << std::endl;
                std::cout << "tmp.header.stamp.nsec: " << tmp.header.stamp.nsec << std::endl;
                std::cout << "tmp.header.frameId: " << tmp.header.frameId << std::endl;
                dataSkeleton->mdcEvent.Send(std::move(tmp));
                //100ms sleep 10hz
                //usleep(interval * 1000);
                if (comPara.frequency != "") {
                    int32_t freq = atoi(comPara.frequency.c_str());
                    int32_t sleepTime = 1000000 / freq;
                    usleep(sleepTime);
                } else {
                    usleep(1000000 / 30);//30hz
                }
            }
        } while (isLoop);
    }
private:
    void GetFilesFromDirectory(const std::string &name, std::vector<std::string> &v)
    {
        DIR *dirp = opendir(name.c_str());
        if (dirp == nullptr) {
            return;
        }
        struct dirent *dp;
        while ((dp = readdir(dirp)) != nullptr) {
            if (strcmp(dp->d_name, ".") == 0 || strcmp(dp->d_name, "..") == 0) {
                continue;
            }
            if (EndsWith(dp->d_name, ".pcd")) {
                v.push_back(dp->d_name);
            }
        }
        closedir(dirp);
    }

    bool EndsWith(const std::string &str, const std::string &suffix)
    {
        return str.size() >= suffix.size() && str.compare(str.size() - suffix.size(), suffix.size(), suffix) == 0;
    }
};

#endif // PCD_SENDER_H
